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Abstract — Real-time  tracking  of  human  body  motion  is  an  im¬ 
portant  technology  in  synthetic  environments,  robotics,  and  other 
human-computer  interaction  applications.  This  paper  presents 
an  extended  Kalman  filter  designed  for  real-time  estimation  of 
the  orientation  of  human  limb  segments.  The  filter  processes  data 
from  small  inertial/magnetic  sensor  modules  containing  triaxial 
angular  rate  sensors,  accelerometers,  and  magnetometers.  The 
filter  represents  rotation  using  quaternions  rather  than  Euler 
angles  or  axis/angle  pairs.  Preprocessing  of  the  acceleration  and 
magnetometer  measurements  using  the  Quest  algorithm  produces 
a  computed  quaternion  input  for  the  filter.  This  preprocessing 
reduces  the  dimension  of  the  state  vector  and  makes  the  mea¬ 
surement  equations  linear.  Real-time  implementation  and  testing 
results  of  the  quaternion-based  Kalman  filter  are  presented. 
Experimental  results  validate  the  filter  design,  and  show  the 
feasibility  of  using  inertial/magnetic  sensor  modules  for  real-time 
human  body  motion  tracking. 

Index  Terms — Inertial  sensors,  Kalman  filtering,  magnetic  sen¬ 
sors,  motion  measurement,  orientation  tracking,  pose  estimation, 
quaternions,  virtual  reality. 


I.  Introduction 

MOTION  tracking  is  a  key  technology  in  synthetic  envi¬ 
ronments,  robotics,  and  other  applications  that  require 
real-time  information  about  the  motion  of  a  human.  A  number 
of  motion-tracking  technologies  have  been  developed  for  human 
motion  capture  in  virtual  reality  and  biomedical  applications, 
including  mechanical  trackers,  active  magnetic  trackers,  optical 
tracking  systems,  acoustic,  and  inertial/magnetic  tracking  sys¬ 
tems.  Most  are  dependent  on  an  artificially  generated  source  and 
are  thus  range-limited  and  susceptible  to  interference  and  noise. 

Mechanical  tracking  systems  can  be  placed  in  two  separate 
categories.  Body-based  systems  use  an  exoskeleton  that  is  at¬ 
tached  to  the  articulated  structure  to  be  tracked  [1],  Goniometers 
within  the  skeletal  linkages  measure  joint  angles.  Ground-based 
systems  attach  one  end  of  a  boom  or  shaft  to  a  tracked  object  and 
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typically  have  six  degrees  of  freedom  (DOFs)  [2],  Ground-based 
systems  normally  only  track  a  single  rigid  body,  but  have  the  ad¬ 
vantage  of  being  able  to  provide  haptic  feedback. 

Practical  optical  tracking  systems  can  also  be  separated  into 
two  basic  categories.  Pattern  recognition  systems  sense  an  arti¬ 
ficial  pattern  of  lights  and  use  this  information  to  determine  po¬ 
sition  and/or  orientation  [3],  Such  systems  may  be  “outside-in” 
when  the  sensors  are  fixed  and  the  emitters  are  mobile,  or  “in- 
side-out”  when  sensors  are  mounted  on  mobile  objects  and  the 
emitters  are  fixed.  Image-based  systems  determine  position  by 
using  multiple  cameras  to  track  predesignated  points  on  moving 
objects  within  a  working  volume.  The  tracked  points  may  be 
marked  actively  or  passively  [4],  [5], 

Active  magnetic  tracking  systems  determine  both  position 
and  orientation  by  using  sets  of  small  orthogonally  mounted 
coils  to  sense  a  set  of  sequentially  generated  magnetic  fields. 
The  sequentially  emitted  fields  induce  current  in  each  of  the 
sensor  coils,  allowing  measurement  of  orientation.  Changes  in 
total  strength  across  the  sensor  coils  are  proportional  to  the  dis¬ 
tance  from  the  field  transmitter  and  can  be  used  to  measure  po¬ 
sition  [6]. 

Ultrasonic  tracking  systems  can  determine  position  through 
either  time-of-flight  and  triangulation  or  phase-coherence. 
Phase-coherence  trackers  determine  distance  by  measuring  the 
difference  in  phase  of  a  reference  signal  and  an  emitted  signal 
detected  by  sensors. 

Body  tracking  using  inertial  and  magnetic  sensors  is  a  rela¬ 
tively  new  technology.  Inertial/magnetic  tracking  is  appealing 
due  to  a  lack  of  dependence  on  an  artificially  generated  source. 
It  thus  does  not  suffer  from  range  limitations  and  interference 
problems  of  sourced  technologies.  All  delay  or  latency  is  due  to 
data  processing  and  transmission.  The  availability  of  low-cost, 
small-size  micro-electro-mechanical  systems  (MEMS)  sensors 
has  made  it  possible  to  build  wrist-watch-sized,  self-contained 
inertial/magnetic  sensor  modules  [7],  [8].  These  modules  can 
be  used  to  accurately  track  orientation  in  real  time.  Attachment 
of  such  sensor  modules  to  each  of  the  major  limb  segments  of 
a  human  makes  it  possible  to  independently  determine  the  ori¬ 
entation  of  each  segment  relative  to  an  Earth-fixed  reference 
frame.  The  human  model  is  constructed  from  multiple  indepen¬ 
dently  oriented  limb  segments  that  are  constrained  by  their  at¬ 
tachment  to  each  other.  Relative  orientation  between  limb  seg¬ 
ments  is  not  determined  or  needed. 

A  naive  approach  to  inertial  orientation  tracking  might  in¬ 
volve  integration  of  angular  rate  data  to  determine  orientation. 
However,  this  solution  would  be  prone  to  drift  over  time  due  to 
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the  buildup  of  bias  and  drift  errors.  In  order  to  avoid  drift,  in¬ 
ertial  tracking  systems  make  use  of  additional  complementary 
sensors.  Commonly,  these  sensors  include  triads  of  accelerome¬ 
ters  and  magnetometers  for  respectively  referencing  the  gravity 
and  magnetic  field  vectors.  Measuring  the  gravity  vector  in  the 
sensor  coordinate  frame  using  accelerometers  allows  estimation 
of  orientation  relative  to  the  horizontal  plane.  However,  in  the 
event  that  the  sensor  module  is  rotated  about  the  vertical  axis,  the 
projection  of  the  gravity  vector  on  each  of  the  principal  axes  of 
the  accelerometer  triad  will  not  change.  Since  the  accelerometer 
triad  can  not  be  used  to  sense  a  rotation  about  the  vertical  axis, 
magnetometers  are  used  to  measure  the  local  magnetic  field 
vector  in  sensor  coordinates  and  allows  determination  of  ori¬ 
entation  relative  to  the  vertical.  The  data  from  the  incorporated 
sensors  is  normally  fused  using  a  Kalman  or  complementary 
filtering  algorithm.  It  should  be  noted  that  data  from  low-cost 
MEMS  accelerometers  cannot  be  double-integrated  for  an  ex¬ 
tended  period  of  time  to  determine  position,  due  to  a  quadratic 
growth  of  errors. 

This  paper  describes  the  design,  implementation,  and  experi¬ 
mental  testing  of  an  extended  Kalman  filter  (EKF)  for  real-time 
tracking  of  human  body  motion.  In  order  to  produce  3-D  orien¬ 
tation  estimates  relative  to  an  Earth-fixed  reference  frame,  the 
filter  uses  input  data  from  a  sensor  module  containing  a  triad  of 
orthogonally  mounted  linear  accelerometers,  a  triad  of  orthogo¬ 
nally  mounted  angular  rate  sensors,  and  a  triad  of  orthogonally 
mounted  magnetometers.  Quaternions  are  used  to  represent  ori¬ 
entation  to  improve  computational  efficiency  and  avoid  singu¬ 
larities.  In  addition,  the  use  of  quaternions  eliminates  the  need 
for  computing  trigonometric  functions.  The  filter  continuously 
corrects  for  drift  based  on  the  assumption  that  human  limb  ac¬ 
celeration  is  bounded,  and  averages  to  zero  over  any  extended 
period  of  time.  A  first-order  linear  system  is  used  to  model 
human  body  limb  segment  motion.  The  QUEST  algorithm  is 
used  to  preprocess  accelerometer  and  magnetometer  measure¬ 
ments,  resulting  in  a  significant  simplification  of  the  Kalman 
filter  design.  The  filter  is  experimentally  validated  using  actual 
sensor  measurements. 

The  primary  contributions  of  this  paper  are: 

•  analysis  that  determines  that  a  simple  motion  model  based 
on  a  first-order  linear  system  is  sufficient  for  tracking 
human  limb  segment  orientation; 

•  an  EKF  designed  for  tracking  human  limb  segment  orien¬ 
tation  that  fuses  a  precalculated  quaternion  input  with  an¬ 
gular  rate  data; 

•  experimental  results  validating  that  filter  performance  is 
adequate  for  human  posture  tracking  applications. 

The  paper  is  organized  as  follows.  Section  II  provides  an 
overview  of  related  work,  and  contrasts  that  with  the  approach 
described  in  this  paper.  Section  III  gives  a  brief  description  of 
the  MARG  sensors  used  to  obtain  experimental  data.  Section  IV 
presents  the  process  model  of  the  Kalman  filter  for  human  body 
motion  tracking.  Section  V  describes  two  approaches  to  Kalman 
filter  design.  Section  VI  describes  implementation  issues  of  the 
Kalman  filter  with  a  focus  on  how  the  nonlinear  process  model 
was  first  linearized  and  then  discretized.  Experimental  modeling 
of  the  process  noise  covariance  matrix  and  the  measurement 
noise  covariance  matrix  is  also  detailed.  Section  VII  reports  the 


MATLAB  simulation  and  offline  testing  results  of  the  Kalman 
filter.  Section  VIII  describes  the  real-time  implementation  of  the 
algorithm  and  testing  results.  The  final  section  provides  a  sum¬ 
mary  and  conclusions. 

II.  Related  Work 

Many  studies  of  human  motion  tracking  using  inertial  sensors 
have  been  performed.  Depending  on  the  type,  number,  and  con¬ 
figuration  of  sensors  used,  some  studies  are  limited  to  tracking 
two  degrees  of  orientation  in  a  plane,  while  others  track  3-D 
orientation.  Algorithms  have  also  been  designed  to  track  limb 
segment  orientations  relative  to  each  other  or  calculate  joint  an¬ 
gles,  as  opposed  to  estimating  the  orientation  of  a  limb  segment 
relative  to  an  Earth-fixed  reference  frame. 

A  study  of  human  motion  tracking  using  accelerometers 
alone  was  reported  in  [9].  During  motions  involving  small 
linear  accelerations,  a  set  of  triaxial  accelerometers  was  used 
to  determine  joint  angles.  During  motions  accompanied  by 
higher  accelerations,  a  technique  is  described  that  involves  the 
use  of  two  sets  of  triaxial  accelerometers  on  a  single  rigid  body 
to  differentiate  gravitational  acceleration  from  motion-related 
linear  acceleration.  Though  the  effects  of  these  geometric 
sensor  fusion  techniques  are  depicted,  there  is  no  comparison 
with  truth  data.  The  use  of  magnetometers  is  mentioned,  but 
not  discussed.  Rehbinder  and  Hu  [10]  describe  an  attitude 
estimation  algorithm  based  on  the  use  of  angular  rate  sensors 
and  accelerometers.  In  this  paper,  drift  in  heading  estimation 
was  unavoidable  due  to  a  lack  of  additional  complementary 
sensors,  such  as  magnetometers.  Thus,  only  two  DOFs  of 
orientation  are  tracked.  Sabatini  et  al.  [11]  used  a  single  sensor 
module  containing  a  biaxial  accelerometer  and  one  gyroscope 
to  perform  gait  analysis  and  measurement.  To  measure  incline, 
distance,  and  speed,  the  method  exploits  the  cyclical  features 
of  human  gait.  Transition  from  one  gait  phase  to  the  next  is 
determined  using  gyroscope  data.  Acceleration  data  is  double 
integrated  during  the  swing  phase  to  determine  position  and 
used  to  determine  the  vertical  when  the  foot  is  flat  on  the 
ground.  Since  the  accelerometers  are  unable  to  detect  rotations 
about  the  vertical  plane,  all  motion  is  assumed  to  take  place 
in  a  nonrotating  sagittal  plane.  Sabatini  [12]  took  this  research 
further  by  creating  a  quaternion-based  filtering  algorithm. 
A  quaternion  interpolation  technique  is  used  to  improve  the 
accuracy  of  orientation  and  position  estimates  by  reducing 
the  effects  of  sensor  bias  and  scale  factor  drift  in  both  the 
accelerometers  and  gyroscope.  Unlike  the  work  described  in 
this  paper,  this  gait  analysis  work  does  not  attempt  to  measure 
posture.  In  similar  gait  measurement  work,  Veltink  et  al.  [13] 
use  a  sensor  module  containing  a  three-axis  accelerometer  and 
a  three-axis  angular  rate  sensor  to  measure  gait  characteristics 
in  order  to  tune  an  implantable  drop-foot  simulator. 

In  a  study  of  dynamic  registration  in  augmented  reality  ap¬ 
plications  that  require  more  precise  orientation  tracking  as  well 
as  position  tracking,  Azuma  and  Bishop  [14]  use  inertial  data 
from  linear  accelerometers  and  angular  rate  sensors  to  reduce 
apparent  lag  in  the  position  and  orientation  estimates  produced 
by  an  optoelectronic  tracking  system.  The  use  of  an  EKF  pre¬ 
dictor  resulted  in  errors  5-10  times  lower  than  without  predic- 
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tion.  In  contrast,  the  work  described  in  this  paper  produces  only 
estimates  of  orientation  using  inertial  and  magnetic  data. 

Full  3 -DOF  orientation  tracking  is  most  commonly  per¬ 
formed  using  nine-axis  sensor  modules  containing  three 
orthogonally  mounted  triads  of  angular  rate  sensors,  ac¬ 
celerometers,  and  magnetometers.  Foxlin  et  al.  [15],  [16] 
describes  two  commercial  nine-axis  sensing  systems  designed 
for  head  tracking  applications.  Sensor  fusion  is  performed  using 
a  complementary  separate-bias  Kalman  filter.  Drift  correction 
is  described  as  only  being  performed  during  stationary  periods 
when  it  is  assumed  accelerometers  are  sensing  only  gravita¬ 
tional  acceleration.  Thus,  the  described  algorithm  requires  that 
all  motion  stop  in  order  to  correct  inertial  drift  errors. 

Bachmann  et  al.  [7],  [17]  proposed  a  nonoptimal  quaternion- 
based  complementary  filter  for  human  body  tracking.  The  filter 
is  able  to  track  through  all  orientations  without  singularities,  and 
continuously  correct  for  drift  without  a  need  for  stationary  pe¬ 
riods  using  nine-axis  inertial/magnetic  sensor  module  data.  Ex¬ 
tensions  to  this  work  and  the  development  of  an  optimal  filter  de¬ 
signed  for  human  posture  tracking  applications  are  described  in 
[  1 8]— [20] .  Use  of  a  first-order  linear  system  for  modeling  human 
body  limb  motions  was  first  proposed  in  [18].  A  Gauss-Newton 
iteration  method  is  used  to  preprocess  accelerometer  and  mag¬ 
netometer  data  to  produce  quaternion  input  to  the  EKF.  Formu¬ 
lation  and  simulation  testing  of  a  reduced-order  implementation 
of  the  Gauss-Newton  iteration  method  for  this  Kalman  filter  is 
documented  in  [19].  Preliminary  experimental  testing  results  are 
presented  in  [20]. 

In  [21],  Gallagher  et  al.  present  a  nonoptimal  complemen¬ 
tary  filter  algorithm  that  has  a  lower  computational  complexity 
and  similar  accuracy  to  the  work  described  by  Bachmann  et 
al.  in  [7]  and  [17].  Luinge  describes  a  Kalman  filter  designed 
for  human  body  tracking  application  in  [22]— [24].  In  the  pro¬ 
posed  method,  inclination  is  determined  without  low-pass  fil¬ 
tering  accelerometer  data.  The  design  is  based  on  assumptions 
concerning  the  frequency  content  of  the  acceleration  and  the 
magnitude  of  gravity.  Reduction  of  drift  about  the  vertical  axis 
is  dependent  on  the  use  of  a  kinematic  human  body  model.  Mag¬ 
netometers  are  not  used.  More  recently,  Roetenberg  et  al.  [25] 
extended  the  Kalman  filter  described  in  [23]  to  include  a  mag¬ 
netometer  model  designed  to  prevent  heading  drift  and  com¬ 
pensate  for  magnetic  disturbances.  This  compensation  allowed 
a  significant  estimation  accuracy  improvement  in  comparison 
with  no  compensation  or  using  angular  rate  sensors  only.  In 
[26],  Zhu  and  Zhou  describe  a  linear  Kalman  filter  algorithm 
designed  to  smooth  accelerometer  and  magnetometer  readings 
from  a  nine-axis  sensor  module.  Rather  than  estimating  indi¬ 
vidual  limb  segment  orientations  relative  to  a  fixed  reference 
frame,  as  is  done  in  this  paper,  their  system  determines  joint 
angles  in  axis/angle  form  using  the  data  from  the  two  sensors 
mounted  on  the  inboard  and  outboard  sides  of  the  joint.  The  axis/ 
angle  pairs  are  determined  analytically  using  processed  mea¬ 
surement  data. 

Kraft  [27]  describes  an  “unscented,”  quaternion-based 
Kalman  filter  for  real-time  estimation  of  rigid-body  orientation 
using  nine-axis  sensor  modules.  The  described  filter  approx¬ 
imates  the  Gaussian  probability  distribution  using  a  set  of 
sample  points  instead  of  linearizing  nonlinear  process  model 
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equations.  Simulation  results  demonstrate  the  general  validity 
of  the  described  filter.  Tests  of  the  filter  with  real  measure¬ 
ments  are  mentioned,  but  not  shown  or  quantified.  Haid  and 
Breitenbach  [28]  also  describe  a  Kalman  filter  algorithm  for 
use  with  inertial  and  magnetic  sensors.  The  primary  aim  of 
the  filter  is  the  elimination  of  drift  and  bias  effects  observed 
in  low-cost  angular  rate  sensors.  The  filter  works  only  in  the 
single  dimension  of  the  targeted  angular-rate  sensor.  It  does  not 
estimate  limb  segment  orientation  or  joint  angles. 

Some  work  has  attempted  to  eliminate  the  need  to  include  an¬ 
gular  rate  sensors  in  inertial/magnetic  sensor  modules.  In  [29], 
Gebre-Egziabher  et  al.  describe  an  attitude  determination  al¬ 
gorithm  for  aircraft  applications.  The  algorithm  is  based  on  a 
quaternion  formulation  of  Wahba’ s  problem  [30] ,  where  magne¬ 
tometer  and  accelerometer  measurements  are  used  to  determine 
attitude  without  the  use  of  angular  rate  sensors.  A  Kalman  filter 
implementation  of  the  algorithm  is  also  presented.  The  algo¬ 
rithm  is  based  on  the  assumption  that  the  rigid  body  to  which  the 
sensor  is  attached  is  stationary  or  is  slow  moving,  and  is  thus  not 
applicable  to  highly  dynamic  tracking  applications.  Chin- Woo 
et  al.  [31]  propose  a  gyroscope  free  inertial  navigation  system 
that  uses  accelerometers  to  determine  both  linear  and  angular 
motions  of  a  rigid  body.  The  approach  requires  a  minimum  of 
six  accelerometers.  Acceptable  configurations  and  basic  algo¬ 
rithms  are  examined  through  simulation.  Use  of  accelerome¬ 
ters  to  calculate  angular  rate  results  in  a  faster  orientation  error 
growth  rate  than  that  associated  with  conventional  angular  rate 
sensors.  This  result  is  due  to  inclusion  of  the  angular  accelera¬ 
tion  terms  which  introduce  integrated  noise  and  drift.  The  idea 
of  using  accelerometers  to  measure  angular  rate  is  carried  fur¬ 
ther  by  Ang  et  al.  in  [32]  and  [33], 

In  contrast  with  the  work  described  above,  this  paper  presents 
a  filter  algorithm  that  is  specifically  designed  for  tracking 
human-limb  segment  orientation  relative  to  an  Earth-fixed 
frame.  The  algorithm  incorporates  a  human  body  motion 
model.  It  adopts  a  two-layer  filter  architecture,  in  which  the 
QUEST  algorithm  preprocesses  accelerometer  and  magne¬ 
tometer  data  and  an  EKF  fuses  the  QUEST  output  with  angular 
rate  data. 

III.  MARG  Sensors 

Experimental  data  were  collected  using  MARG  III  iner¬ 
tial/magnetic  sensor  modules  designed  by  the  authors  and 
fabricated  by  McKinney  Technology  [8].  The  MARG  sensor 
design  is  based  on  its  primary  application,  that  is,  human  body 
motion  tracking.  Primary  sensing  components  for  this  unit 
include  Tokin  CG-L43  ceramic  rate  gyros.  Analog  Devices 
ADXL202E  micromachined  accelerometers,  and  Honeywell 
HMC1051Z  and  HMC1052  one-  and  two-axis  magnetometers. 
The  sensor  module  also  incorporates  a  Texas  Instruments 
MSP430F149  ultra-low-power,  16-bit  RISC  architecture  mi¬ 
crocontroller.  Overall,  dimensions  of  the  MARG  III  unit  are 
approximately  1.8  cmx3.0  cmx2.5  cm. 

The  manufacturer  specified  maximum  allowable  angular  rate 
of  the  CG-L43  ceramic  gyro  is  ±  90°/s.  This  is  deemed  suffi¬ 
cient  to  quicken  response  in  human  body  motion  tracking  appli¬ 
cations,  but  not  accurately  measure  rates  associated  with  highly 
dynamic  motion.  Three  of  these  gyros  are  orthogonally  mounted 
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Fig.  1.  Kalman  filter  process  model,  q  is  the  orientation  quaternion,  to  is  the  angular  velocity,  w  is  a  white  noise,  and  r  is  the  time  constant. 


within  the  MARG  unit  to  form  a  triad  capable  of  measuring 
3-DOF  angular  rate.  In  the  results  presented  here,  the  limita¬ 
tions  of  the  angular  rate  sensors  did  not  affect  the  performance 
or  demonstration  of  the  algorithms  during  typical  human  mo¬ 
tion. 

The  maximum  measurement  range  of  Analog  Devices 
ADXL202E  is  ±  2  g,  which  is  acceptable  for  sensing  gravita¬ 
tional  acceleration.  The  ADXL202E  is  a  two-axis  acceleration 
sensor  on  a  single  chip.  As  a  result,  only  two  of  them  are  re¬ 
quired  to  form  a  triad  for  measuring  3-DOF  acceleration.  The 
ADXL202E  also  offers  a  duty  cycle  output,  which  can  be  directly 
interfaced  to  a  low-cost  microcontroller  without  analog/digital 
(A/D)  converters.  The  accelerometers  are  not  used  to  measure 
linear  accelerations  associated  with  human  motion. 

In  the  MARG  design,  a  one-axis  HMC1051Z  for  the  z-axis 
and  a  two-axis  HMC1052  for  x-y  axes  are  mounted  on  the  same 
PCB  to  form  a  three-axis  magnetometer.  The  HMC1051Z  and 
HMC1052  are  specially  designed  to  be  mounted  on  the  same 
PCB  to  form  an  orthogonal  triad. 

The  purpose  of  the  microcontroller  is  to  convert  analog  sensor 
outputs  to  digital  data,  digitally  filter  the  angular  rate  sensor 
data,  and  perform  automatic  set/reset  of  magnetometers  to  avoid 
magnetic  saturation  problems.  To  prepare  angular  rate  data  for 
processing  by  the  Kalman  filter,  the  data  is  averaged  on  start 
up  to  establish  initial  bias  values.  During  run-time,  angular  rate 
data  is  preprocessed  using  a  simple  first-order  high-pass  filter 
to  eliminate  drift  over  time.  Static  bench  tests  have  established 
that  accelerometer  and  magnetometer  data  are  relatively  stable 
over  time,  and  they  are  thus  not  bias-corrected  at  run-time.  Mag¬ 
netic  interference  is  a  major  concern  when  using  magnetome¬ 
ters  in  environments  containing  changing  or  distorted  magnetic 
fields  [34].  This  is  an  active  area  of  research  by  the  authors  and 
others  [25].  No  compensation  for  external  magnetic  effects  is 
performed  in  the  work  described  in  this  paper.  It  is  noted  that  the 
MARG  sensor  is  a  prototype  constructed  using  2  g  accelerom¬ 
eters  and  90°/s  angular  rate  sensors.  These  components  were 
chosen  for  typical  human  motion,  and  may  not  be  sufficient  for 
extreme  human  motion.  The  algorithm  presented  in  the  paper  is 
not  limited  to  these  particular  sensor  parameters. 

IV.  Kalman  Filter  Process  Model 

As  stated  above,  the  objective  of  this  paper  is  to  design  a 
Kalman  filter  for  real-time  tracking  of  human  body  motion.  To 
do  so,  it  is  necessary  to  establish  a  process  model  representing 
motion  dynamics  of  the  human  musculoskeletal  system.  Dy¬ 
namic  models  of  the  human  musculoskeletal  systems  are  com¬ 
plex,  and  have  been  studied  for  many  years.  Such  models  are 
ideal  for  computer  simulations  of  articulated  body  motions,  but 
remain  too  computationally  demanding  for  real-time  applica¬ 
tions  such  as  real-time  human  motion  tracking.  Thus,  the  chal¬ 
lenge  is  to  develop  a  model  that  is  simple  yet  adequate  for  mo¬ 
tion  tracking  applications.  Based  on  extensive  trial  and  error 


study,  a  first-order  linear  system  model  is  adopted  to  represent 
the  motion  of  each  human  body  limb  segment.  Such  a  model  is 
depicted  in  the  left  half  of  Fig.  1.  It  is  assumed  that  each  limb 
segment  is  independent  of  the  others.  The  input  to  the  linear 
system  is  a  white  noise  w ,  and  the  output  is  the  angular  velocity 
u>  of  the  limb  segment.  The  most  important  parameter  in  this 
model  is  the  time  constant  r,  which  determines  how  fast  a  limb 
segment  (e.g.,  upper  arm)  can  move  in  typical  human  motion 
conditions.  The  angular  velocity  is  thus  modeled  as  a  colored 
noise  generated  by  a  linear  system  with  a  white  noise  input. 

In  the  filter,  quaternions  are  used  to  represent  the  orientation 
of  each  body  limb  segment  for  two  reasons.  First,  the  quaternion 
representation  does  not  suffer  from  the  singularity  problem  as¬ 
sociated  with  the  Euler  angle  representation.  Second,  it  avoids 
trigonometric  functions  in  the  filter  algorithm,  making  it  more 
efficient  and  easier  to  implement  in  real  time  on  microcon¬ 
trollers.  In  what  follows,  q  will  be  used  to  denote  the  orientation 
quaternion  in  Earth  coordinates.  The  angular  velocity  u  and  the 
quaternion  derivative  q  are  related  by  the  following  well-known 
identity  [35]: 


where  X  represents  quaternion  multiplication.  Equation  (1)  is 
represented  by  the  center  block  in  Fig.  1 .  The  quaternion  deriva¬ 
tive  q  is  integrated  to  produce  the  quaternion  q.  In  order  to  take 
advantage  of  computational  simplifications  and  efficiencies  as¬ 
sociated  with  unit  quaternions,  the  resultant  quaternion  is  nor¬ 
malized  to  unit  length  in  the  last  step  of  the  process  model,  as 
shown  in  Fig.  1.  The  quaternion  q  produced  by  the  integrator 
may  not  be  exactly  unit  length,  but  it  is  normally  very  close  to  a 
unit  quaternion.  To  avoid  the  complexity  that  the  normalization 
introduces  into  the  Kalman  filter  derivation,  it  is  not  included  in 
the  process  model  equations  presented  in  the  next  section.  As  a 
result,  although  the  Kalman  filter  is  an  optimal  algorithm,  this 
normalization  procedure  leads  to  a  suboptimal  algorithm.  In  the 
next  section,  two  Kalman  filter  designs  based  on  this  process 
model  will  be  presented. 

V.  Kalman  Filter  Design 

Two  alternative  approaches  to  the  Kalman  filter  design  based 
on  the  process  model  presented  in  Section  IV  will  be  described 
in  this  section.  The  state  vector  for  both  approaches  is  the  same. 
It  is  a  7-D  vector  consisting  of  the  three  components  of  angular 
rate  and  the  four  elements  of  the  orientation  quaternion.  The 
difference  between  the  two  approaches  is  in  the  measurement 
or  output  equation  for  the  Kalman  filter.  The  first  approach  uses 
a  standard  Kalman  filter  design,  which  has  a  9-D  measurement 
vector,  consisting  of  3-D  angular  rate,  3-D  acceleration,  and  3-D 
local  magnetic  field.  This  9-D  vector  directly  corresponds  to 
the  measurements  provided  by  inertial/magnetic  sensors  mod¬ 
ules.  The  first  three  components  of  the  output  equation  (angular 
rate  portion)  are  linearly  related  to  the  state  vector.  However,  the 
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other  six  components  of  the  output  equation  are  nonlinearly  re¬ 
lated  to  the  state  vector.  The  nonlinear  relationship  is  quite  com¬ 
plicated.  As  a  result,  the  EKF  designed  with  this  output  equation 
is  computationally  inefficient. 

The  second  approach  uses  a  separate  algorithm  to  find  a 
corresponding  quaternion  for  each  set  of  accelerometer  and 
magnetometer  measurements.  The  computed  quaternion  is  then 
combined  with  the  angular  rate  measurements,  and  presented  to 
the  Kalman  filter  as  its  measurements.  By  doing  so,  the  output 
equations  for  the  Kalman  filter  become  linear,  and  the  overall 
Kalman  filter  design  is  greatly  simplified. 

A.  The  First  Approach 

The  first  approach  is  a  standard  Kalman  filter  design  based  on 
the  process  model  depicted  in  Fig.  1.  The  state  vector  x  is  7-D, 
with  the  first  three  components  being  the  angular  rate  uj,  and  the 
last  four  components  being  the  quaternion  q.  That  is 
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Fig.  2.  Block  diagram  of  the  first  approach  to  Kalman  filter  design. 
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Fig.  3.  Block  diagram  of  the  second  approach  to  Kalman  filter  design. 


X\ 

U>1 

x2 

= 

u>2 

_x3  _ 

_U)3_ 

"214" 

'Qi~ 

2:5 

<12 

X& 

<13 

-X7_ 

-<14- 

Based  on  Fig.  1 ,  the  state  equations  are  given  by 


to  I—1 

1 

/ 

to  I—1 

+ 

Ja. 

~  X  4.  ~ 

xr, 

T 

1 

~X4~ 

X5 

_X3 

-  0  - 

211 

CO  E- 

“  2 

Xq 

-X7  _ 

1 

CN  CO 

(2) 

(3) 


It  is  noted  that  quaternion  normalization  is  not  modeled  in  these 
state  equations,  but  is  carried  out  in  the  real-time  implementa¬ 
tion. 

Since  measurement  data  to  the  filter  are  provided  by  MARG 
sensors,  it  is  natural  to  choose  the  following  as  the  measure¬ 
ments  of  the  Kalman  filter: 


Zl 

x  component  of  angular  rate 

Z2 

= 

y  component  of  angular  rate 

>3. 

z  component  of  angular  rate 

Z4 

x  component  of  acceleration 

Z5 

= 

y  component  of  acceleration 

_Z6_ 

z  component  of  acceleration 

z7 

x  component  of  local  magnetic  field 

Z3 

= 

y  component  of  local  magnetic  field 

_zg 

z  component  of  local  magnetic  field 

Since  angular  rates  are  part  of  the  state,  the  first  three  measure¬ 
ment  equations  are  simply  given  by  the  following: 


z\  ~  J-\  ~  G  (4) 

z2=  x2  +  V2  (5) 

Z3=%3  +  V3  (6) 

where  vt  is  the  measurement  noise  that  is  assumed  to  be  white. 
As  for  the  remaining  six  measurement  equations,  they  turn  out  to 


be  quite  complicated.  As  an  example,  the  seventh  measurement 
equation  is  given  by 

z7  =  ((2:4  +  x~  -  x\  -  xl)  /hi  +  2(x±X5  -  xex7)/h2 

+2(x4Xe  +  X5X7)/h3)  /  (214  +  x\  +  21§2lf)  +  V4  (7) 

where  h\,h2,  and  //,->  are  values  of  the  Earth  magnetic  field 
measured  in  the  Earth  coordinates,  which  are  constant  for  a 
given  location.  It  is  not  difficult  to  design  an  EKF,  as  shown 
in  Fig.  2,  based  on  state  (2)  and  (3),  and  the  nine  measurement 
equations,  which  was  indeed  carried  out  in  [36].  The  problem  is 
that  computational  requirements  for  implementing  such  a  filter 
are  extremely  high,  making  it  unfeasible  for  real-time  motion 
tracking.  An  alternative  approach  to  the  Kalman  filter  design  is 
thus  presented  in  the  next  subsection. 

B.  The  Second  Approach 

Fig.  3  shows  a  block  diagram  of  an  alternative  approach  to 
filter  design.  Acceleration  and  local  magnetic  field  measure¬ 
ments  are  used  as  input  to  the  QUEST  algorithm  [37]  to  produce 
what  will  be  called  the  computed  quaternion.  The  computed 
quaternion  together  with  angular  rate  measurements  is  then  pre¬ 
sented  to  a  Kalman  filter  as  measurements.  It  will  be  seen  below 
that  the  Kalman  filter  in  this  case  is  significantly  simpler,  owing 
to  the  fact  that  the  measurement  equations  are  linear.  It  is  true 
that  there  is  an  additional  computational  cost  to  implement  the 
QUEST  algorithm  in  this  approach.  Still,  the  overall  computa¬ 
tional  requirements  for  this  approach  are  much  less  than  what  is 
needed  for  the  first  approach. 

The  QUEST  (quaternion  estimator)  algorithm  is  a  popular  al¬ 
gorithm  for  a  single-frame  estimation  of  an  attitude  quaternion 
[37].  The  algorithm  was  created  to  solve  Wahba’s  problem  [30] 
that  involved  determination  of  the  attitude  of  a  rigid  body  in  ref¬ 
erence  to  a  fixed  coordinate  system  based  on  a  set  of  measure¬ 
ment  or  observation  vectors  using  a  closed  form  solution.  The 
minimum  number  of  measurement  vectors  required  to  compute 
orientation  is  two.  Early  solutions  to  Wahba’s  problem  directly 
compute  a  rotation  matrix  capable  of  rotating  the  measurement 
(assuming  no  errors)  vectors  to  match  the  reference  vectors.  The 
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QUEST  algorithm  solves  Wahba’s  problem  by  calculating  the 
four  elements  of  the  corresponding  optimal  quaternion  [37]. 

This  alternative  approach  to  filter  design  as  shown  in  Fig.  3 
is  not  without  reasons.  If  the  limb  segment  to  which  an  iner¬ 
tial/magnetic  sensor  module  is  attached  is  stationary,  accelera¬ 
tion  (gravity)  and  local  magnetic  held  measurements  are  suffi¬ 
cient  to  determine  the  orientation  of  the  body.  While  stationary, 
accelerometers  measure  the  local  gravity  vector  in  the  body 
frame.  The  3-D  gravity  measurements  can  be  used  to  determine 
roll  and  pitch  angles  of  the  body  relative  to  the  fixed  Earth  frame. 
The  yaw  angle  of  the  body  is  determined  from  the  local  mag¬ 
netic  held  measurements.  In  this  application,  the  QUEST  al¬ 
gorithm  takes  gravity  and  magnetic  held  measurement  vectors 
with  equal  weight  and  computes  the  optimal  quaternion  that  will 
rotate  these  vectors  to  match  their  corresponding  reference  vec¬ 
tors. 

While  the  rigid  body  is  in  motion,  the  computed  quaternions 
from  this  algorithm  do  not  represent  the  actual  real-time  ori¬ 
entation  of  the  body,  because  accelerometers  measure  the  sum 
of  gravity  and  motion  induced  acceleration.  This  is  where  an¬ 
gular  rate  measurements  come  to  help  estimate  the  orientation 
of  the  rigid  body.  While  angular  rate  measurements  can  be  in¬ 
tegrated  to  yield  an  orientation  estimate,  they  are  prone  to  drift 
over  an  extended  period  of  time.  Acceleration  and  magnetic  held 
measurements  do  not  drift  over  time.  The  Kalman  hlter  in  this 
approach  is  designed  to  optimally  fuse  the  complementary  in¬ 
formation  provided  by  the  angular  rate  measurements  and  the 
computed  quaternion. 

It  should  be  pointed  out  that  this  hltering  architecture  has 
been  previously  proposed  and  successfully  applied  in  other 
areas  such  as  attitude  heading  and  reference  systems  (AHRS) 
[38],  In  [38],  an  inertial  navigation  system  for  autonomous 
underwater  vehicles  was  developed,  in  which  a  complementary 
hlter  hrst  combines  measurement  data  from  accelerometers, 
angular  rate  sensors,  and  magnetic  sensors.  An  EKF  then  fuses 
the  output  of  the  complementary  hlter  with  the  GPS/DGPS 
measurements. 

The  state  equations  in  the  second  approach  are  the  same  as 
those  in  the  hrst  approach,  that  is,  equations  (2)  and  (3).  The 
measurement  equations  in  this  case  are  much  simpler,  and  they 
are 


Zi  =  Xi+Vi,  i  =  (8) 

where  Vi  is  the  white  noise  measurement.  Although  the  mea¬ 
surement  equations  are  linear,  an  EKF  is  still  required  since  the 
second  part  of  the  state  (3)  is  nonlinear.  Nevertheless,  linearity 
in  the  measurement  equations  signihcantly  simplihes  the  hlter 
design  and  reduces  computational  requirements  for  real-time 
implementation. 

C.  Discussion 

The  hrst-order  process  model  and  an  early  version  of  the 
second  approach  to  the  Kalman  hlter  design  was  hrst  reported  in 
[18].  Rather  than  using  the  QUEST  algorithm,  a  Gauss-Newton 
iteration  method  was  used  to  preprocess  accelerometer  and 
magnetometer  data  to  produce  quaternion  input  to  the  EKF.  A 


reduced-order  implementation  of  the  Gauss-Newton  iteration 
method  was  described  in  [19].  This  reduced  order  implemen¬ 
tation  requires  computing  the  inverse  of  a  3x3  matrix  rather 
than  that  of  a  4x4  matrix.  The  Gauss-Newton  iteration  method 
was  replaced  by  the  factored  quaternion  algorithm  in  [20]. 
While  more  efficient,  the  factored  quaternion  algorithm  pro¬ 
vides  a  suboptimal  solution.  The  QUEST  algorithm  provides 
an  optimal  solution  for  noisy  measurement  data.  Preliminary 
experimental  results  were  also  reported  in  [20].  In  this  paper, 
the  QUEST  algorithm  is  adopted  to  preprocess  the  acceleration 
and  magnetic  held  measurement  data.  The  QUEST  algorithm 
requires  computing  the  inverse  of  a  4x4  matrix,  but  it  is  a 
single-frame  or  noniterative  algorithm.  The  QUEST  algorithm 
needs  to  be  executed  once  for  each  sampling  step  of  the  Kalman 
hlter.  The  Gauss-Newton  method  needs  to  be  iteratively  evalu¬ 
ated  several  times  until  it  converges  for  each  sampling  step  of 
the  Kalman  hlter. 

VI.  Kalman  Filter  Implementation 

In  this  section,  the  implementation  of  the  second  approach 
Kalman  hlter  design  will  be  described.  First,  the  state  equations 
are  linearized  and  discretized  to  yield  a  discrete  process  model. 
Second,  modeling  of  the  process  noises  and  measurement  noises 
is  presented. 

The  state  equations  (2)  and  (3)  can  be  written  together  in  the 
following  form: 

X  =  f(x)  +  w(t).  (9) 

This  nonlinear  process  model  can  be  linearized  along  the  cur¬ 
rently  estimated  trajectory  x 

Ax  -  la=8  Art;  +  w(t).  (10) 

The  actual  trajectory  x  is  the  sum  of  the  estimated  trajectory  x 
and  the  small  increment  Ax 

x  =  x  +  Ax.  (11) 

The  next  step  is  to  convert  the  continuous-time  model  (10)  into  a 
discrete-time  model.  Let  S  =  At  be  the  sampling  interval.  Then 
the  difference  equation  corresponding  to  the  differential  (10)  is 
given  by 

Axk+1  =  $fcAa;fc  +  wk  (12) 

where  the  discrete  state  transition  matrix  is 
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Fig.  4.  Block  diagram  of  the  EKF. 


and  wk  is  a  vector  of  discrete  white  process  noise  and  its  ele¬ 
ments  are  given  by 

{+  4-h-t 

Jtk+1e  Wi(rf)drf,  *  =  1,2,3  (14) 

0,  i  =  4, 5, 6,  7. 

The  measurement  (8)  are  linear  and  thus  linearization  is  not 
required.  The  corresponding  discrete  measurement  equation  is 
given  by 

zk  =  Hkxk+vk  (15) 

where  Hk  is  the  7x7  identity  matrix.  An  EKF  can  now  be  de¬ 
signed  for  the  discrete  process  (12)  and  the  discrete  measure¬ 
ment  ( 15).  A  complete  diagram  of  the  filter  is  depicted  in  Fig.  4. 
It  is  seen  from  Fig.  4  that  the  model  parameters  <frk,Hk,  Rk,  and 
Qk  need  to  be  provided  to  start  the  filter.  <T>j.  is  the  discrete  state 
transition  matrix  given  by  (13).  Hk  is  the  identity  measurement 
equation  matrix  of  (15).  The  determination  of  the  covariance 
matrix  Qk  of  the  process  noises  and  the  covariance  matrix  Rk 
of  the  measurement  noises  is  discussed  below. 

The  process  noise  covariance  matrix  Qk  is  defined  by 

Qk  =  E[wkwk  ]  (16) 

where  E  is  the  expectation  operator,  and  wk  is  the  discrete 
process  white  noise  vector  of  (12),  whose  components  are  given 
by  (14).  Before  computing  Qk,  it  should  be  noted  that  the  con¬ 
tinuous  process  noises  w(t)  =  [wi(t),w2(t),W3(t)]T  of  the 
state  (2)  are  independent  white  noises  with  zero  mean  and  vari¬ 
ance  D, .  As  such,  the  covariance  is  given  by 

^K(i)«;J-(S)]  =  {^(<_4  i  =  (17) 


Simulated  angular  rates  Actual  angular  rates  from  MARG  sensor 


0  5  10  15  20  0  5  10  15  20 


0  5  10  15  20  0  5  10  15  20 


Time  (second) 


Time  (second) 


Fig.  5.  Comparison  of  the  simulated  angular  rate  (left)  and  actual  angular  rate 
measurements  (right). 


Using  (17)  and  (14),  the  process  noise  covariance  matrix  Qk  of 
(16)  is  evaluated  to  be 

'  q\ -|  0  0  0  0  0  0" 

0  Q22  0  0  0  0  0 

0  0  q33  0  0  0  0 

Qk=  0  0  00000  (18) 

0  0  0  0  0  0  0 

0  0  0  0  0  0  0 

.0  0  0  0  0  0  0. 

where  qn ,  q22,  and  q 33  are  given  by 

qu  =  E[w ifcWifc]  =  (i  -  e-~)  (19) 

q22  =  E[w2kw2k\  =  (l  _  e  T2  )  (2°) 

933  =  E[w3kw3k]  =  (l  -  e_^)  •  (21) 

What  remains  to  be  determined  are  the  variance  D,  of  the  con¬ 
tinuous  white  noise  processes  and  the  time  constant  t.;  of  the 
process  model.  They  are  determined  using  actual  measurement 
data  from  the  MARG  sensors  and  a  Matlab  simulation  imple¬ 
menting  the  angular  rate  process  model  (2).  The  variance  and 
time  constant  in  the  simulation  are  adjusted  until  the  output  of 
the  simulation  closely  matches  the  actual  measurement  data.  For 
this  purpose,  a  MARG  sensor  was  attached  to  the  right  lower 
arm  of  a  user  and  typical  arm  motion  data  were  collected.  It  was 
experimentally  determined  that  rt  =  0.5  s,  Di  =  0.4  rad2/s2. 

Fig.  5  shows  a  comparison  between  the  simulated  angular 
rates  and  the  actual  angular  rates  obtained  from  a  MARG  III 
sensor  for  typical  arm  motions.  The  graphs  to  the  left  represent 
the  angular  rates  generated  by  the  simulation  model.  The  graphs 
to  the  right  are  the  angular  rates  measured  by  a  MARG  sensor. 
It  can  be  observed  that  the  two  sets  of  data  exhibit  similar  char¬ 
acteristics.  Autocorrelations  of  the  simulated  and  actual  x-axis 
angular  rate  data  are  plotted  in  Fig.  6.  The  autocorrelation  of  the 
actual  angular  rate  data  obtained  from  the  MARG  sensor  was 
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Autocorrelation  of  the  simulated  (top)  and  actual  (bottom)  angular  rates 


Fig.  6.  Autocorrelations  of  the  simulated  x-axis  angular  rate  (top  plot)  and  the 
actual  x-axis  angular  rate  (bottom  plot). 

first  computed.  The  parameters  of  the  process  model  were  then 
adjusted  so  that  the  autocorrelation  of  the  simulated  angular  rate 
closely  matches  that  of  the  actual  data.  It  is  seen  that  they  are 
not  exactly  the  same,  but  closely  resemble  each  other. 

The  measurement  noise  covariance  matrix  Rp,  represents  the 
level  of  confidence  placed  in  the  accuracy  of  the  measurements, 
and  is  given  by 

Rk  =  E[vkv[].  (22) 

In  principle,  Rf,  is  not  necessarily  diagonal.  For  practical  pur¬ 
poses,  only  diagonal  elements  are  experimentally  determined 
based  on  actual  measurements.  A  MARG  sensor  was  placed  in 
various  static  configurations,  and  data  were  collected.  The  vari¬ 
ances  of  the  first  three  measurement  components  are  determined 
directly  from  angular  rate  measurements,  and  the  variances  of 
the  other  four  components  (quaternion  components)  are  deter¬ 
mined  using  computed  quaternions.  The  experimentally  deter¬ 
mined  values  are  Ru  =  R2 2  =  R33  =  0.01  rad2/s-2,  and 
R44  —  R^r,  —  R(,c,  —  R77  —  0.0001. 

VII.  Offline  MATLAB  Testing  Results 

After  deriving  all  the  required  parameters  to  initialize  the 
Kalman  filter,  it  was  implemented  using  MATLAB  to  test  the 
performance  and  accuracy  of  the  quaternion  orientation  esti¬ 
mates.  Real  world  data  recorded  using  a  MARG  sensor  was  used 
in  these  tests. 

Since  the  Kalman  gain  was  determined  such  that  the  sum  of 
squared  errors  is  minimized,  one  way  to  measure  the  conver¬ 
gence  of  the  Kalman  filter  is  through  examination  of  the  trace 
of  the  error  covariance  matrix  Pp,.  Fig.  7  shows  the  trace  of  Pp, 
for  the  first  200  samples  of  data  obtained  with  the  sensor  in  its 
reference  position  (x-axis  pointing  north,  y-axis  pointing  east, 
and  z-axis  point  down).  It  is  noted  that  the  sum  of  squared  er¬ 
rors  reaches  a  steady  state  after  approximately  0.6  s. 

Table  I  shows  the  elements  of  the  quaternion  for  the  first  five 
samples.  The  initial  estimate  was  chosen  to  be  the  unit  quater- 


Fig.  7.  Trace  of  the  error  covariance  matrix. 


TABLE  I 

Convergence  of  the  Quaternion  Estimates 


Sample 

90 

91 

92 

93 

1 

0.99985 

0.0082135 

0.0066032 

0.013570 

2 

0.99991 

0.0057585 

0.0049037 

0.011901 

3 

0.99990 

0.0055983 

0.0048826 

0.011882 

4 

0.99990 

0.005288 

0.0046884 

0.011784 

5 

0.99990 

0.0052297 

0.0046353 

0.011506 

nion  (0.5,  0.5,  0.5,  0.5).  The  actual  position  of  the  sensor  in  the 
reference  position  is  represented  by  the  quaternion  (1,  0,  0,  0). 
The  data  shown  in  Table  I  indicates  that  the  Kalman  filter  esti¬ 
mate  converged  to  the  actual  position  in  a  single  iteration. 

While  the  QUEST  algorithm  works  well  for  static  orientation 
and  slow  movements,  the  objective  of  the  Kalman  filter  is  to 
blend  angular  rate  measurements  with  the  estimates  produced 
using  magnetometer  and  accelerometer  data  during  periods  in 
which  the  sensor  module  is  subjected  to  motions  involving  high 
angular  rates  and  large  linear  accelerations.  To  verify  the  esti¬ 
mation  accuracy  during  such  periods,  the  orientation  estimates 
of  the  Kalman  filter  were  compared  with  the  estimates  produced 
using  only  the  QUEST  algorithm  with  no  rate  measurement  and 
with  the  reference  motion  of  a  precision  tilt  table.  Two  kinds 
of  experiments  were  conducted  for  this  test.  The  first  used  con¬ 
trolled  rotations  produced  by  a  HAAS  precision  tilt  table.  The 
table  has  two  DOFs  and  is  capable  of  positioning  to  an  accuracy 
of  0.001°  at  rates  ranging  from  0.001  to  80°/s.  In  order  to  mit¬ 
igate  any  possible  magnetic  effects  generated  by  the  steel  con¬ 
struction  of  the  tilt  table,  the  sensor  package  was  mounted  on 
a  nonferrous  extension  above  the  table  as  shown  in  Fig.  8.  The 
extension  is  made  of  a  piece  of  PVC  pipe  and  is  approximately 
1  m  in  length.  The  second  experiment  used  a  random  motion 
pattern  produced  while  the  sensor  was  attached  to  the  arm  of  a 
person. 

In  the  first  set  of  experiments,  the  sensor  was  initially  placed 
with  its  xyz  axes  aligned  with  north-east-down  directions.  The 
sensor  was  rotated  —90°  about  the  x-axis  at  a  rate  of  60°/s  and 
then  rotated  90°  at  the  same  rate  (in  the  reverse  direction)  for 


1224 


IEEE  TRANSACTIONS  ON  ROBOTICS,  VOL.  22,  NO.  6,  DECEMBER  2006 


Fig.  8.  Experimental  setup  using  a  HAAS  precision  tilt  table. 


Estimated  vs.  reference  roll  trajectory 


Time  (seconds) 


Fig.  10.  Zoom-in  view  of  the  roll  estimate  (solid  curve)  from  the  Kalman  filter 
and  the  tilt  table  reference  motion  (dashed  curve)  with  a  90°  rotation  in  roll. 


Estimate  from  QUEST  algorithm  Estimate  from  Kalman  filter  Difference  between  the  estimated  and  reference  roll  trajectories 


Sample  number  Sample  number  7  8  9  10  11  12  13  14  15 

Time  (second) 

Fig.  9.  Orientation  estimate  produced  by  the  QUEST  algorithm  (left)  and  the  Fig.  1 1 .  Difference  between  the  roll  estimate  and  the  tilt  table  reference  motion. 
Kalman  filter  (right)  with  a  90°  rotation  in  roll  axis. 


two  cycles.  Fig.  9  shows  the  performance  of  the  Kalman  filter 
in  estimating  the  orientation  of  the  sensor.  The  graphs  to  the 
left  show  the  orientation  estimated  by  the  QUEST  algorithm, 
and  the  graphs  to  the  right  show  the  orientation  estimated  by 
the  Kalman  filter.  It  can  be  seen  that  the  QUEST  algorithm  was 
able  to  correctly  estimate  the  roll  angle  before  the  first  (negative) 
rotation,  between  the  first  and  second  (positive)  rotations,  and 
after  the  second  rotation,  but  it  is  not  able  to  correctly  estimate 
orientation  during  the  rotational  motions.  During  the  rotational 
motions,  the  accelerometers  measure  the  sum  of  gravity  and  mo¬ 
tion  induced  acceleration.  Without  rate  sensors,  the  QUEST  al¬ 
gorithm  is  not  able  to  differentiate  gravity  from  the  motion  ac¬ 
celeration.  Relatively  large  errors  in  pitch  and  yaw  were  also 
produced  by  the  QUEST  algorithm.  On  the  other  hand,  it  can 
be  seen  from  the  top-right  plot  that  the  Kalman  filter  was  able 
to  correctly  estimate  the  roll  angle  throughout  the  duration  of 
the  experiment.  The  small  pitch  and  yaw  motions  seen  in  the 
center-right  and  bottom-right  plots  are  due  to  misalignment  of 


the  sensor  module  with  the  motion  axes  of  the  experimental  tilt 
table.  The  misalignments  were  corrected  manually,  but  could 
not  be  completely  removed  without  the  use  of  equipment  not 
available  to  the  authors.  To  confirm  that  the  errors  were  due 
to  misalignments,  the  algorithm  was  tested  using  synthetically 
generated,  noise-free  data  with  rotation  in  only  one  axis.  These 
results  demonstrated  that  the  algorithm  does  not  produce  any 
observable  cross-coupling  responses  in  other  axes. 

To  illustrate  the  accuracy  of  the  Kalman  filter,  the  estimates 
produced  by  the  Kalman  filter  can  be  compared  with  the  mo¬ 
tion  of  the  tilt  table.  Since  the  tilt  table  used  in  the  experiments 
is  much  more  accurate  than  the  tracking  system  under  evalua¬ 
tion,  its  motion  can  be  treated  as  a  truth  reference.  In  Fig.  10, 
the  top-right  plot  of  Fig.  9  is  replotted  in  a  zoom-in  view  for  the 
time  period  of  7-15  s.  The  solid  curve  represents  the  roll  esti¬ 
mate  from  the  Kalman  filter,  and  the  dashed  curve  is  the  refer¬ 
ence  trajectory  of  the  tilt  table.  The  difference  between  these  two 
curves  is  shown  in  Fig.  1 1.  It  is  observed  from  Figs.  10  and  1 1 
that  the  static  accuracy  of  the  filter  is  better  than  2°  for  the  time 
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Sample  number  Sample  number 

Fig.  12.  Orientation  estimate  produced  by  the  QUEST  algorithm  (left)  and  the 
Kalman  filter  (right)  with  a  45°  rotation  in  pitch  axis. 


Fig.  14.  Snapshot  of  real-time  testing.  The  user  with  two  MARG  sensors  at¬ 
tached  to  the  right  arm  is  the  foreground  and  the  human  avatar  projected  on  a 
screen  is  in  background. 


Estimate  from  QUEST  algorithm  Estimate  from  Kalman  filter 


Time  (seconds)  Time  (seconds) 

Fig.  13.  Orientation  estimate  produced  by  the  QUEST  algorithm  (left)  and  the 
Kalman  filter  (right)  with  random  arm  movements. 


Fig.  15.  Another  snapshot  of  real-time  testing. 


periods  of  about  7-9.8  s  and  1 1.3-15  s.  During  the  time  period 
of  9.8-1 1.3  s,  the  tilt  table  and  the  MARG  sensor  are  in  the  dy¬ 
namic  state  moving  from  —90.0°  to  0.0°  at  the  rate  of  60°/s.  It 
can  be  observed  from  Fig.  1 1  that  the  maximum  error  is  about  9°. 
This  large  dynamic  error  is  mainly  due  to  the  lag  of  the  tracking 
system.  The  lag  is  on  the  order  of  100  ms,  as  depicted  by  the 
horizontal  gap  between  the  blue  curve  and  green  curve  during 
the  time  period  of  10-1 1  s.  The  sampling  rate  is  100  Hz,  which 
yields  a  lag  of  10  ms.  The  computational  time  required  to  ex¬ 
ecute  the  filter  algorithm  is  about  1.6  ms.  The  additional  lag  is 
caused  by  data  transmission.  In  human  body  tracking  applica¬ 
tions,  this  lag-induced  error  is  only  observable  during  highly 
dynamic  motion,  and  is  not  of  great  enough  magnitude  to  im¬ 
pair  user  interaction  with  a  virtual  environment. 

Fig.  12  shows  plots  of  rotating  the  sensor  about  the  y-axis  first 
by  45°  and  then  by  —45°  at  a  rate  of  45°/s.  Similar  results  are 
observed  in  this  experiment. 

Fig.  13  shows  the  results  of  an  experiment  in  which  the  sensor 
was  rotated  randomly  while  attached  to  the  arm  of  a  person.  Al¬ 


though  there  is  no  true  reference  in  this  case,  it  can  be  seen  that 
the  Kalman  filter  eliminated  the  jittering  and  spiking  contained 
in  the  orientation  estimates  produced  by  using  the  QUEST  al¬ 
gorithm  alone. 

VIII.  Real-Time  Testing  Results 

After  initial  testing  of  the  EKF  with  the  MATLAB  implemen¬ 
tation,  the  QUEST  algorithm  and  EKF  algorithm  were  imple¬ 
mented  in  Java  for  real-time  testing  and  evaluation.  Computa¬ 
tion  time  required  to  perform  a  single  update  is  1 .6  ms.  Memory 
management  in  the  Java  implementation  is  carefully  performed 
to  avoid  the  requirement  for  garbage  collection  and  possible 
interruption  of  filter  processing.  The  real-time  quaternion  pro¬ 
duced  by  the  Kalman  filter  was  visualized  using  a  human-like 
avatar  as  seen  in  Figs.  14  and  15.  Two  MARG  sensors  were  used 
to  track  the  motion  of  a  human  arm,  one  sensor  being  attached  to 
the  upper  arm  and  the  other  attached  to  the  lower  arm.  A  video 
clip  demonstrating  real-tracking  of  human  arm  motions  is  avail¬ 
able  at  http://ieeexplore.ieee.org. 
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The  QUEST  algorithm  was  able  to  track  the  motion  of  the 
human  arm  under  slow-moving  conditions  where  linear  accel¬ 
eration  was  not  significant.  However,  when  the  arm  motion  be¬ 
came  faster,  the  algorithm  was  not  able  to  follow  the  arm  motion, 
resulting  in  observable  lag  as  well  as  overshoots. 

When  the  EKF  was  integrated  with  the  QUEST  algorithm,  the 
avatar  was  able  to  successfully  track  the  human  arm  motion  in 
real  time  under  all  conditions.  Furthermore,  the  filtering  process 
did  not  produce  any  noticeable  lag.  Movement  of  the  human  arm 
and  the  avatar  was  synchronized. 

IX.  Conclusion 

This  paper  presents  the  design,  implementation,  and  experi¬ 
mental  results  of  a  quaternion-based  Kalman  filter  for  real-time 
human  body  motion  tracking  using  inertial/magnetic  sensor 
modules  containing  orthogonally  mounted  triads  of  accelerom¬ 
eters,  angular  rate  sensors,  and  magnetometers.  This  subject 
filter  is  not  applicable  to  applications  in  which  accelerations 
due  to  forces  other  than  gravity  are  present  for  indefinite  pe¬ 
riods.  The  filter  was  designed  with  the  goal  of  being  able  to 
produce  highly  accurate  orientation  estimates  in  real  time.  This 
real-time  requirement  precluded  the  use  of  complex  models 
of  human  motion.  Instead  the  filter  design  makes  use  of  a 
simple  first-order  linear  system  model.  Output  of  the  model 
is  angular  velocity  modeled  as  colored  noise  generated  from 
white  noise  input.  The  Kalman  filter  design  is  further  simplified 
by  preprocessing  accelerometer  and  magnetometer  data  using 
the  single-frame  QUEST  algorithm.  The  quaternion  produced 
by  QUEST  is  provided  as  input  to  the  Kalman  filter  along  with 
angular  rate  data.  In  comparison  to  more  traditional  approaches, 
this  preprocessing  step  significantly  reduces  the  complexity 
of  filter  design  by  allowing  the  use  of  linear  measurement 
equations.  Prior  to  testing  of  the  filter  algorithm,  values  for 
variances  and  time  constants  where  determined  by  comparing 
simulation  results  to  actual  measurement  data  obtained  during 
typical  arm  motions.  This  process  was  considered  complete 
when  the  autocorrelation  of  the  simulation  data  closely  matched 
that  of  the  actual  data.  In  experiments  designed  to  validate  filter 
performance,  this  approach  was  shown  to  work  well.  In  these 
experiments,  filter  orientation  estimates  were  compared  with 
truth  data  obtained  from  a  rotary  tilt  table.  Filter  response  very 
closely  matched  tilt  table  motion  with  a  static  accuracy  better 
than  2°  and  a  dynamic  accuracy  of  better  than  9°.  This  larger 
error  during  motion  was  largely  caused  by  data  communication 
delays.  Even  with  this  delay,  qualitative  experiments  in  which 
the  algorithm  was  used  demonstrate  that  these  dynamic  errors 
were  not  of  great  enough  magnitude  to  impair  user  interaction 
with  a  virtual  environment. 

The  Kalman  filter  design  presented  in  this  paper  is  the  result 
of  several  years  of  effort.  With  refinement  of  this  design  and 
others  mentioned  in  the  related  work  section,  orientation  esti¬ 
mation  algorithms  have  reached  a  limit  given  the  accuracy  and 
noise  characteristics  of  the  MEMs  sensors  employed  in  the  ap¬ 
plication.  The  angular  rate  sensors  and  accelerometers  are  truly 
“sourceless”  and  do  not  depend  on  any  outside  reference.  How¬ 
ever,  though  it  is  not  artificially  generated,  the  magnetometers 
must  sense  a  homogenous  ambient  magnetic  field  in  order  for 
these  systems  to  deliver  orientation  estimates  that  are  stable  in 
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azimuth.  Thus  the  ultimate  accuracy  of  these  algorithms  can  not 
be  determined  by  considering  only  the  sensors  and  the  imple¬ 
mented  algorithms. 
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